//#########################################################################
//                              WackiEngie
//           Copyright (c) 2014 Marc Wacker. All rights reserved.
//#########################################################################

#include "WeQuaternion.h"

namespace WackiEngine
{


	template<class T>
	Quaternion<T>::Quaternion(T x_, T y_, T z_, T w_)
		: x(x_), y(y_), z(z_), w(w_)
	{
	}

	template<class T>
	Quaternion<T>::Quaternion(const Matrix3<T> &m)
	{
		set(m);
	}

	template<class T>
	Quaternion<T>::Quaternion(T angle, const Vector3<T> &axis)
	{
		set(angle, axis);
	}
	template<class T>
	void Quaternion<T>::set(const Matrix3<T> &m)
	{
		// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
		// article "Quaternion Calculus and Fast Animation".

		const int next[3] = { 1, 2, 0 };

		T trace = m.trace();
		T root;

		if(trace > (T)0)
		{
			// |_w| > 1/2, may as well choose _w > 1/2
			root = sqrt(trace + (T)1);  // 2w
			w = ((T)0.5)*root;
			root = ((T)0.5) / root;       // 1/(4w)
			x = (m(2, 1) - m(1, 2)) * root;
			y = (m(0, 2) - m(2, 0)) * root;
			z = (m(1, 0) - m(0, 1)) * root;
		}
		else
		{
			// |w| <= 1/2
			int i = 0;
			if(m(1, 1) > m(0, 0)) i = 1;
			if(m(2, 2) > m(i, i)) i = 2;
			int j = next[i];
			int k = next[j];

			root = sqrt(m(i, i) - m(j, j) - m(k, k) + (T)1);
			T* quat[3] = { &x, &y, &z };
			*quat[i] = ((T)0.5)*root;
			root = ((T)0.5) / root;
			w = (m(k, j) - m(j, k))*root;
			*quat[j] = (m(j, i) + m(i, j))*root;
			*quat[k] = (m(k, i) + m(i, k))*root;
		}
	}

	template<class T>
	void Quaternion<T>::set(T angle, const Vector3<T> &axis)
	{
		T angleRAD = angle * DEG2RAD;
		w = (T)cos(angleRAD * (T)0.5);
		x = y = z = (T)sin(angleRAD * (T)0.5);
		x *= axis.x;
		y *= axis.y;
		z *= axis.z;
	}

	template<class T>
	void Quaternion<T>::interpolate(Quaternion<T> &out, const Quaternion<T> &start, const Quaternion<T> &end, const T factor)
	{
		// calc cosine theta
		T cosom = start.x * end.x + start.y * end.y + start.z * end.z + start.w * end.w;

		// adjust signs (if necessary)
		Quaternion<T> endCpy = end;
		if(cosom < static_cast<T>(0.0))
		{
			cosom = -cosom;
			endCpy.x = -endCpy.x;   // Reverse all signs
			endCpy.y = -endCpy.y;
			endCpy.z = -endCpy.z;
			endCpy.w = -endCpy.w;
		}

		// Calculate coefficients
		T sclp, sclq;
		if((static_cast<T>(1.0) - cosom) > static_cast<T>(0.0001)) // 0.0001 -> some epsillon
		{
			// Standard case (slerp)
			T omega, sinom;
			omega = acos(cosom); // extract theta from dot product's cos theta
			sinom = sin(omega);
			sclp = sin((static_cast<T>(1.0) - factor) * omega) / sinom;
			sclq = sin(factor * omega) / sinom;
		}
		else
		{
			// Very close, do linear interp (because it's faster)
			sclp = static_cast<T>(1.0) - factor;
			sclq = factor;
		}

		out.x = sclp * start.x + sclq * endCpy.x;
		out.y = sclp * start.y + sclq * endCpy.y;
		out.z = sclp * start.z + sclq * endCpy.z;
		out.w = sclp * start.w + sclq * endCpy.w;
	}

	template<class T>
	inline Matrix3<T> Quaternion<T>::getMatrix() const
	{
		Matrix3<T> resMatrix(
			static_cast<T>(1.0) - static_cast<T>(2.0) * (y * y + z * z),
			static_cast<T>(2.0) * (x * y - z * w),
			static_cast<T>(2.0) * (x * z + y * w),
			static_cast<T>(2.0) * (x * y + z * w),
			static_cast<T>(1.0) - static_cast<T>(2.0) * (x * x + z * z),
			static_cast<T>(2.0) * (y * z - x * w),
			static_cast<T>(2.0) * (x * z - y * w),
			static_cast<T>(2.0) * (y * z + x * w),
			static_cast<T>(1.0) - static_cast<T>(2.0) * (x * x + y * y));

		return resMatrix;
	}

}